// $Id: adaptica.cpp 76 2012-07-20 18:03:52Z alan.watson.f $

#include <errno.h>
#include <math.h>
#include <string.h>
#include <time.h>

#include "oaxaca-config.h"
#include "oaxaca.h"
#include "adaptica.h"

#include <mirrorDriver.h>
#include <mirrorDriverExtras.h>

static mirrorDriver *m = NULL;

#define WITH_HARDWARE 1

static double vmode[OAXACA_WFC_NMODE][OAXACA_WFC_NV];

static void
initialize_modes(void)
{
  double pi = 4.0 * atan(1.0);

  for (int imode = 0; imode < OAXACA_WFC_NMODE; ++imode)
    for (int iv = 0; iv < OAXACA_WFC_NV; ++iv)
      vmode[imode][iv] = 0.0;
      
  // The tilt electrodes in order with origin 1.
  int fr_tilt_electrode[] = {
    7, 5, 3, 1, 16, 14, 12, 10, 39, 37, 35, 48, 46, 44, 42
  };
  int bk_tilt_electrode[] = {
    56, 27, 18, 22, 26, 30, 2, 6, 41, 45, 51, 55, 59, 63, 52
  };

  // mode 0 is x-tilt
  for (int i = 0; i < 15; ++i) {
    float theta = (i + 0.5) * 2.0 * pi / 15.0;
    float v = cos(theta);
    if (v > 0)
      vmode[0][fr_tilt_electrode[i] - 1] = +v;
    else
      vmode[0][bk_tilt_electrode[i] - 1] = -v;
  }
    
  // mode 1 is y-tilt
  for (int i = 0; i < 15; ++i) {
    float theta = (i + 0.5) * 2.0 * pi / 15.0;
    float v = sin(theta);
    if (v > 0)
      vmode[1][fr_tilt_electrode[i] - 1] = +v;
    else
      vmode[1][bk_tilt_electrode[i] - 1] = -v;
  }      

}

void
oaxaca_adaptica_initialize(void)
{
  if (m != NULL)
    oaxaca_error("while initializing: the controller is already initialized.");
  m = new mirrorDriver();  
  initialize_modes();
#if WITH_HARDWARE
  char ip_address[] = OAXACA_ADAPTICA_IP_ADDRESS;
  if (!m->configIPAddress(ip_address))
    oaxaca_error("while initializing: unable to configure the ip address.");
  if (!m->connectToMirror())
    oaxaca_error("while initializing: unable to connect to \"%s\".", ip_address);
#else
  fprintf(stdout, "lots of messages to stdout.\n");
  fprintf(stderr, "lots of messages to stderr.\n");
  sleep(5);
#endif
}

void
oaxaca_adaptica_set_voltages()
{
  if (m == NULL)
    oaxaca_error("while setting voltages: the controller has not been initialized.");

  double v[OAXACA_WFC_NV];

  if (oaxaca_shm->wfc.modal) {
  
    for (int iv = 0; iv < OAXACA_WFC_NV; ++iv)
      v[iv] = 0.0;

    // The tilt electrodes in order.
    int fr_tilt_electrode[] = {
      7, 5, 3, 1, 16, 14, 12, 10, 39, 37, 35, 48, 46, 44, 42
    };
    int bk_tilt_electrode[] = {
      56, 27, 18, 22, 26, 30, 2, 6, 41, 45, 51, 55, 59, 63, 52
    };

    double pi = 4.0 * atan(1.0);
  
    for (int j = 0; j < 15; ++j) {
      float theta = (j + 0.5) * 2.0 * pi / 15.0;
      float vmode = oaxaca_shm->wfc.mode[0] * cos(theta);
      if (vmode > 0)
        v[fr_tilt_electrode[j] - 1] = +vmode;
      else
        v[bk_tilt_electrode[j] - 1] = -vmode;
    }
    for (int j = 0; j < 15; ++j) {
      float theta = (j + 0.5) * 2.0 * pi / 15.0;
      float vmode = oaxaca_shm->wfc.mode[1] * sin(theta);
      if (vmode > 0)
        v[fr_tilt_electrode[j] - 1] = +vmode;
      else
        v[bk_tilt_electrode[j] - 1] = -vmode;
    }
    
  } else {
  
    for (int iv = 0; iv < OAXACA_WFC_NV; ++iv)
    v[iv] = oaxaca_shm->wfc.v[iv];

  }

  for (int iv = 0; iv <= OAXACA_WFC_NV; ++iv)
    v[iv] = fmin(1.0, fmax(0.0, v[iv]));
  
  for (int iv = 0; iv < OAXACA_WFC_NV; ++iv)
    oaxaca_shm->wfc.v[iv] = v[iv];

#if WITH_HARDWARE
  float f[OAXACA_WFC_NV];
  for (int iv = 0; iv < OAXACA_WFC_NV; ++iv)
    f[iv] = v[iv];
  if (!m->setChannels(f))
    oaxaca_error("while setting voltages: unable to set the voltages.");
#else
  oaxaca_usleep(800);
#endif
}
